/**************************************************************************
作者：caidx1
功能：命令控制器，命令词识别结果转化为对应的执行动作
**************************************************************************/
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/String.h>
#include <iostream>
#include <stdio.h>
#include <joint.h>
#include <std_msgs/Int8.h>
#include <std_msgs/Int32.h>
#include <geometry_msgs/PoseStamped.h>
#include <stdlib.h>
#include <std_msgs/Float64.h>
#include <unistd.h>


using namespace std;
ros::Publisher vel_pub;    //创建底盘运动话题发布者
ros::Publisher cmd_vel_pub;
ros::Publisher goal_control_pub;
ros::Publisher follow_flag_pub;    //创建寻找声源标志位话题发布者
ros::Publisher cmd_vel_flag_pub;    //创建底盘运动控制器标志位话题发布者
ros::Publisher awake_flag_pub;    //创建唤醒标志位话题发布者
ros::Publisher navigation_auto_pub;    //创建自主导航目标点话题发布者
geometry_msgs::Twist cmd_msg;    //底盘运动话题消息数据
geometry_msgs::PoseStamped target;    //导航目标点消息数据
int voice_flag = 0;    //寻找标志位
int goal_control=0;
ros::Publisher ggg_pub;
std::string sw = "on";//标志位区分开关

float I_position_x ;
float I_position_y ;
float I_orientation_z ;
float I_orientation_w ;

float J_position_x ;
float J_position_y ;
float J_orientation_z ;
float J_orientation_w ;

float K_position_x ;
float K_position_y ;
float K_orientation_z ;
float K_orientation_w ;

float Y_position_x ;
float Y_position_y ;
float Y_orientation_z ;
float Y_orientation_w ;

float Z_position_x ;
float Z_position_y ;
float Z_orientation_z ;
float Z_orientation_w ;

float W_position_x ;
float W_position_y ;
float W_orientation_z ;
float W_orientation_w ;

float R_position_x ;
float R_position_y ;
float R_orientation_z ;
float R_orientation_w ;

float S_position_x ;
float S_position_y ;
float S_orientation_z ;
float S_orientation_w ;

float T_position_x ;
float T_position_y ;
float T_orientation_z ;
float T_orientation_w ;

float U_position_x ;
float U_position_y ;
float U_orientation_z ;
float U_orientation_w ;

float V_position_x ;
float V_position_y ;
float V_orientation_z ;
float V_orientation_w ;

float AA_position_x ;
float AA_position_y ;
float AA_orientation_z ;
float AA_orientation_w ;

float BB_position_x ;
float BB_position_y ;
float BB_orientation_z ;
float BB_orientation_w ;

float CC_position_x ;
float CC_position_y ;
float CC_orientation_z ;
float CC_orientation_w ;

float DD_position_x ;
float DD_position_y ;
float DD_orientation_z ;
float DD_orientation_w ;

float EE_position_x ;
float EE_position_y ;
float EE_orientation_z ;
float EE_orientation_w ;

float FF_position_x ;
float FF_position_y ;
float FF_orientation_z ;
float FF_orientation_w ;

float GG_position_x ;
float GG_position_y ;
float GG_orientation_z ;
float GG_orientation_w ;


float line_vel_x ;
float ang_vel_z ;
float turn_line_vel_x ;
 

 
// 四个回调函数，分别处理四个话题接收到的消息
void serialData1Callback(const std_msgs::Float64::ConstPtr& msg) {

	if(msg->data == 2 )
	{
		target.pose.position.x = J_position_x;
		target.pose.position.y = J_position_y;
		target.pose.orientation.z =J_orientation_z;
		target.pose.orientation.w =J_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);
        
        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 2;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKA.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
        
		cout<<"好的：小车自主导航至微电子实验室"<<endl;
		
	}


/***********************************
指令：小车去V点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(msg->data == 1 )
	{
		target.pose.position.x = I_position_x;
		target.pose.position.y = I_position_y;
		target.pose.orientation.z = I_orientation_z;
		target.pose.orientation.w = I_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 1;
        goal_control_pub.publish(goal_control_flag_msg);
       /*system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKB.wav");*/
		cout<<"好的：小车自主导航至V点"<<endl;
	}
/***********************************
指令：小车去I点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(msg->data == 3 )
	{
		target.pose.position.x = K_position_x;
		target.pose.position.y = K_position_y;
		target.pose.orientation.z = K_orientation_z;
		target.pose.orientation.w = K_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 3;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKHD.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
       //system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKHD.wav");
		cout<<"好的：小车自主导航至点"<<endl;
	}
/***********************************
指令：小车去K点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(msg->data == 4 )
	{
		target.pose.position.x = Y_position_x;
		target.pose.position.y = Y_position_y;
		target.pose.orientation.z = Y_orientation_z;
		target.pose.orientation.w = Y_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 4;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKB.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
        //system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKB.wav");
		cout<<"好的：小车自主导航至K点"<<endl;
	}

/***********************************
指令：小车去Y点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(msg->data == 5 )
	{
		target.pose.position.x = Z_position_x;
		target.pose.position.y = Z_position_y;
		target.pose.orientation.z = Z_orientation_z;
		target.pose.orientation.w = Z_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 5;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKHD.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
        //system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKHD.wav");
		cout<<"好的：小车自主导航至Y点"<<endl;
	}

/***********************************
指令：小车去Z点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(msg->data == 6 )
	{
		target.pose.position.x = W_position_x;
		target.pose.position.y = W_position_y;
		target.pose.orientation.z = W_orientation_z;
		target.pose.orientation.w = W_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 6;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKQW.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
        //system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKQW.wav");
		cout<<"好的：小车自主导航至Z点"<<endl;
	}


/***********************************
指令：小车去W点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(msg->data == 7 )
	{
		target.pose.position.x = R_position_x;
		target.pose.position.y = R_position_y;
		target.pose.orientation.z = R_orientation_z;
		target.pose.orientation.w = R_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 7;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKHD.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
        //system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKHD.wav");
		cout<<"好的：小车自主导航至W点"<<endl;
	}

/***********************************
指令：小车去R点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(msg->data == 8 )
	{
		target.pose.position.x = S_position_x;
		target.pose.position.y = S_position_y;
		target.pose.orientation.z = S_orientation_z;
		target.pose.orientation.w = S_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 8;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKB.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
       // system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKB.wav");
		cout<<"好的：小车自主导航至R点"<<endl;
	}

/***********************************
指令：小车去S点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(msg->data == 9 )
	{
		target.pose.position.x = T_position_x;
		target.pose.position.y = T_position_y;
		target.pose.orientation.z = T_orientation_z;
		target.pose.orientation.w = T_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 9;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKQW.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
       // system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKQW.wav");
		cout<<"好的：小车自主导航至S点"<<endl;
	}
/***********************************
指令：小车去T点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(msg->data == 10 )
	{
		target.pose.position.x = U_position_x;
		target.pose.position.y = U_position_y;
		target.pose.orientation.z = U_orientation_z;
		target.pose.orientation.w = U_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 10;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/TYOK.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
        //system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/TYOK.wav");
		cout<<"好的：小车自主导航至T点"<<endl;
	}

/***********************************
指令：小车去AA点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(msg->data == 11 )
	{
		target.pose.position.x = V_position_x;
		target.pose.position.y = V_position_y;
		target.pose.orientation.z = V_orientation_z;
		target.pose.orientation.w = V_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 11;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKB.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
        //system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKB.wav");
		cout<<"好的：小车自主导航至AA点"<<endl;
	}
/***********************************
指令：小车去BB点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(msg->data == 12 )
	{
		target.pose.position.x = AA_position_x;
		target.pose.position.y = AA_position_y;
		target.pose.orientation.z = AA_orientation_z;
		target.pose.orientation.w = AA_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 12;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKQW.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
        //system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKQW.wav");
		cout<<"好的：小车自主导航至BB点"<<endl;
	}

/***********************************
指令：小车去CC点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(msg->data == 13 )
	{
		target.pose.position.x = BB_position_x;
		target.pose.position.y = BB_position_y;
		target.pose.orientation.z = BB_orientation_z;
		target.pose.orientation.w = BB_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 13;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKHD.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
        //system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKHD.wav");
		
                cout<<"好的：小车自主导航至CC点"<<endl;
	     
	}

/***********************************
指令：小车去U点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(msg->data == 14 )
	{
		target.pose.position.x = CC_position_x;
		target.pose.position.y = CC_position_y;
		target.pose.orientation.z = CC_orientation_z;
		target.pose.orientation.w = CC_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 14;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKB.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
       // system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKB.wav");
		cout<<"好的：小车自主导航至U点"<<endl;
	}


/***********************************
指令：小车去DD点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(msg->data == 15 )
	{
		target.pose.position.x = FF_position_x;
		target.pose.position.y = FF_position_y;
		target.pose.orientation.z = FF_orientation_z;
		target.pose.orientation.w = FF_orientation_w;
		navigation_auto_pub.publish(target);
                std_msgs::Int8 goal_control_flag_msg;
                goal_control_flag_msg.data = 15;
                goal_control_pub.publish(goal_control_flag_msg);
				OTHER = (char*) "/feedback_voice/OKHD.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
        //system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKHD.wav");
         sleep(6);
                target.pose.position.x = GG_position_x;
		target.pose.position.y = GG_position_y;
		target.pose.orientation.z = GG_orientation_z;
		target.pose.orientation.w = GG_orientation_w;
                navigation_auto_pub.publish(target);
         sleep(6);
                target.pose.position.x = DD_position_x;
		target.pose.position.y = DD_position_y;
		target.pose.orientation.z = DD_orientation_z;
		target.pose.orientation.w = DD_orientation_w;
		navigation_auto_pub.publish(target);
                std_msgs::Int8 cmd_vel_flag_msg;
                cmd_vel_flag_msg.data = 0;
                cmd_vel_flag_pub.publish(cmd_vel_flag_msg);
		cout<<"好的：小车自主导航至DD点"<<endl;
	}

/***********************************
指令：小车去DD点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(msg->data == 16 )
	{
		target.pose.position.x = EE_position_x;
		target.pose.position.y = EE_position_y;
		target.pose.orientation.z = EE_orientation_z;
		target.pose.orientation.w = EE_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 17;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKHD.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
       // system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKHD.wav");
		cout<<"好的：小车自主导航至DD点"<<endl;
	}

       else if(msg->data == 17 && sw == "on")
	{
		sw = "off";
		OTHER = (char*) "/feedback_voice/OKHD.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
		//system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKHD.wav");
		system("dbus-launch gnome-terminal -- roslaunch simple_follower laserfollow.launch");//打开雷达跟随节点
		cout<<"好的：小车雷达跟随"<<endl;
        }

       else if(msg->data == 18 )
	{
		OTHER = (char*) "/feedback_voice/DJDJ.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
		//system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/DJDJ.wav");
		cout<<"语音打开成功"<<endl;
	}
       else if(msg->data == 19 )
	{   
		OTHER = (char*) "/feedback_voice/OKHD.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
		//system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKHD.wav");
		system("rosnode kill /move_base");
		system("rosnode kill /amcl");
		system("rosnode kill /map_server_for_test");
		system("dbus-launch gnome-terminal -- roslaunch xf_mic_asr_offline voi_navigation.launch");//打开导航节点
		system("dbus-launch gnome-terminal -- roslaunch xf_mic_asr_offline send_mark.launch");//打开多点巡航脚本
		cout<<"好的：小车多点导航"<<endl;
	}

       else if(msg->data == 20 )
	{
		target.pose.position.x = DD_position_x;
		target.pose.position.y = DD_position_y;
		target.pose.orientation.z = DD_orientation_z;
		target.pose.orientation.w = DD_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 15;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKHD.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
        //system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKHD.wav");
		cout<<"好的：小车自主导航至DD点"<<endl;
	}

       else if(msg->data == 21 )
	{
		OTHER = (char*) "/feedback_voice/GG.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
       // system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/GG.wav");
		cout<<"好的：小车自主导航至DD点"<<endl;
	}
}

void serialData2Callback(const std_msgs::Float64::ConstPtr& msg) {
  
}

void serialData3Callback(const std_msgs::Float64::ConstPtr& msg) {
  
}

void serialData4Callback(const std_msgs::Float64::ConstPtr& msg) {
 
}

 
/**************************************************************************
函数功能：离线命令词识别结果sub回调函数
入口参数：命令词字符串  voice_control.cpp等
返回  值：无
**************************************************************************/
void voice_words_callback(const std_msgs::String& msg)
{
	/***指令***/
	string str1 = msg.data.c_str();    //取传入数据
	string str2 = "小车前进";
	string str3 = "小车后退"; 
	string str4 = "小车左转";
	string str5 = "";
	string str6 = "";
	string str7 = "";
	string str8 = "";
	string str9 = "";
	string str10 = "";
	string str11 = "";
	string str12 = "";
	string str13 = "";
	string str14 = "";
	string str15 = "";
	string str16 = "";
	string str17 = "";
	string str18 = "";
	string str19 = "";
	string str20 = "";
	string str21 = "";
	string str22 = "";
	string str23 = "";
	string str24 = "";
	string str25 = "";
	string str26 = "";
	string str27 = "";
	string str28 = "";
	string str29 = "";
        string str30 = "";
        string str37 = "";
        string str38 = "";
        string str39 = "开始干活";
	string str31 = "";
	string str32 = "";
        string str33 = "";
	string str34 = "退下";
        string str35 = "不需要导航";
        string str36 = "去智能充电";
        string str40 = "机械臂向左";
        string str41 = "机械臂向右";
        string str42 = "我要红色方块";
        string str43 = "";
        //string str44 = "小润同学";

/***********************************
指令：小车前进
动作：底盘运动控制器使能，发布速度指令
***********************************/
	if(str1 == str2)
	{
		cmd_msg.linear.x = line_vel_x;
		cmd_msg.angular.z = 0;
		vel_pub.publish(cmd_msg);

		std_msgs::Int8 cmd_vel_flag_msg;
    	cmd_vel_flag_msg.data = 1;
    	cmd_vel_flag_pub.publish(cmd_vel_flag_msg);
    	OTHER = (char*) "/feedback_voice/car_front.wav";//语音反馈
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
		cout<<"好的：小车前进"<<endl;
	}
/***********************************
指令：小车后退
动作：底盘运动控制器使能，发布速度指令
***********************************/
	else if(str1 == str3)
	{
		cmd_msg.linear.x = -line_vel_x;
		cmd_msg.angular.z = 0;
		vel_pub.publish(cmd_msg);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 1;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);
        OTHER = (char*) "/feedback_voice/car_back.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
		cout<<"好的：小车后退"<<endl;
	}
/***********************************
指令：小车左转
动作：底盘运动控制器使能，发布速度指令
***********************************/
	else if(str1 == str4)
	{
		cmd_msg.linear.x = turn_line_vel_x;
		cmd_msg.angular.z = ang_vel_z;
		vel_pub.publish(cmd_msg);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 1;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);
        OTHER = (char*) "/feedback_voice/turn_left.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);        
		cout<<"好的：小车左转"<<endl;
	}
/***********************************
指令：小车右转
动作：底盘运动控制器使能，发布速度指令
***********************************/
	else if(str1 == str5)
	{
		cmd_msg.linear.x = turn_line_vel_x;
		cmd_msg.angular.z = -ang_vel_z;
		vel_pub.publish(cmd_msg);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 1;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);
        OTHER = (char*) "/feedback_voice/turn_right.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
		cout<<"好的：小车右转"<<endl;
	}
/***********************************
指令：小车停
动作：底盘运动控制器失能，发布速度空指令
***********************************/
	else if(str1 == str6)
	{
		cmd_msg.linear.x = 0;
		cmd_msg.angular.z = 0;
		vel_pub.publish(cmd_msg);


		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 1;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);
        OTHER = (char*) "/feedback_voice/stop.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
		cout<<"好的：小车停"<<endl;
	}
/***********************************
指令：小车休眠
动作：底盘运动控制器失能，发布速度空指令，唤醒标志位置零
***********************************/
	else if(str1 == str7)
	{
		cmd_msg.linear.x = 0;
		cmd_msg.angular.z = 0;
		vel_pub.publish(cmd_msg);

		std_msgs::Int8 awake_flag_msg;
        awake_flag_msg.data = 0;
        awake_flag_pub.publish(awake_flag_msg);

        std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 1;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);
        //OTHER = (char*) "/feedback_voice/sleep.wav";
	//WHOLE = join((head + audio_path),OTHER);
	//system(WHOLE);
		cout<<"小车休眠，等待下一次唤醒"<<endl;
	}
/***********************************
指令：小车过来
动作：寻找声源标志位置位
***********************************/
	else if(str1 == str8)
	{
		std_msgs::Int8 follow_flag_msg;
		follow_flag_msg.data = 1;
		follow_flag_pub.publish(follow_flag_msg);
		OTHER = (char*) "/feedback_voice/search_voice.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
		cout<<"好的：小车寻找声源"<<endl;
	}


/***********************************
指令：小车去I点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(str1 == str9)
	{
		target.pose.position.x = J_position_x;
		target.pose.position.y = J_position_y;
		target.pose.orientation.z =J_orientation_z;
		target.pose.orientation.w =J_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);
        
        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 2;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKA.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
       // system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKA.wav");
		cout<<"好的：小车自主导航至微电子实验室"<<endl;
		
	}
/***********************************
指令：小车去I点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(str1 == str10)
	{
		target.pose.position.x = K_position_x;
		target.pose.position.y = K_position_y;
		target.pose.orientation.z = K_orientation_z;
		target.pose.orientation.w = K_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 3;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKHD.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
        //system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKHD.wav");
		cout<<"好的：小车自主导航至点"<<endl;
	}
/***********************************
指令：小车去K点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(str1 == str11)
	{
		target.pose.position.x = Y_position_x;
		target.pose.position.y = Y_position_y;
		target.pose.orientation.z = Y_orientation_z;
		target.pose.orientation.w = Y_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 4;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKB.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
        //system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKB.wav");
		cout<<"好的：小车自主导航至K点"<<endl;
	}

/***********************************
指令：小车去Y点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(str1 == str23)
	{
		target.pose.position.x = Z_position_x;
		target.pose.position.y = Z_position_y;
		target.pose.orientation.z = Z_orientation_z;
		target.pose.orientation.w = Z_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 5;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKHD.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
        //system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKHD.wav");
		cout<<"好的：小车自主导航至Y点"<<endl;
	}

/***********************************
指令：小车去Z点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(str1 == str24)
	{
		target.pose.position.x = W_position_x;
		target.pose.position.y = W_position_y;
		target.pose.orientation.z = W_orientation_z;
		target.pose.orientation.w = W_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 6;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKQW.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
        //system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKQW.wav");
		cout<<"好的：小车自主导航至Z点"<<endl;
	}

/***********************************
指令：小车去W点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(str1 == str25)
	{
		target.pose.position.x = R_position_x;
		target.pose.position.y = R_position_y;
		target.pose.orientation.z = R_orientation_z;
		target.pose.orientation.w = R_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 7;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKHD.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
        //system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKHD.wav");
		cout<<"好的：小车自主导航至W点"<<endl;
	}

/***********************************
指令：小车去R点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(str1 == str26)
	{
		target.pose.position.x = S_position_x;
		target.pose.position.y = S_position_y;
		target.pose.orientation.z = S_orientation_z;
		target.pose.orientation.w = S_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 8;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKB.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
        //system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKB.wav");
		cout<<"好的：小车自主导航至R点"<<endl;
	}

/***********************************
指令：小车去S点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(str1 == str27)
	{
		target.pose.position.x = T_position_x;
		target.pose.position.y = T_position_y;
		target.pose.orientation.z = T_orientation_z;
		target.pose.orientation.w = T_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 9;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKQW.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
        //system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKQW.wav");
		cout<<"好的：小车自主导航至S点"<<endl;
	}

/***********************************
指令：小车去T点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(str1 == str28)
	{
		target.pose.position.x = U_position_x;
		target.pose.position.y = U_position_y;
		target.pose.orientation.z = U_orientation_z;
		target.pose.orientation.w = U_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 10;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/TYOK.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
       // system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/TYOK.wav");
		cout<<"好的：小车自主导航至T点"<<endl;
	}

/***********************************
指令：小车去U点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(str1 == str29)
	{
		target.pose.position.x = CC_position_x;
		target.pose.position.y = CC_position_y;
		target.pose.orientation.z = CC_orientation_z;
		target.pose.orientation.w = CC_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 14;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKB.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
       // system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKB.wav");
		cout<<"好的：小车自主导航至U点"<<endl;
	}

/***********************************
指令：小车去V点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(str1 == str30)
	{
		target.pose.position.x = I_position_x;
		target.pose.position.y = I_position_y;
		target.pose.orientation.z = I_orientation_z;
		target.pose.orientation.w = I_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 1;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKB.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
       /*system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKB.wav");*/
		cout<<"好的：小车自主导航至V点"<<endl;
	}

/***********************************
指令：小车去AA点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(str1 == str31)
	{
		target.pose.position.x = V_position_x;
		target.pose.position.y = V_position_y;
		target.pose.orientation.z = V_orientation_z;
		target.pose.orientation.w = V_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 11;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKB.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
        //system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKB.wav");
		cout<<"好的：小车自主导航至AA点"<<endl;
	}

/***********************************
指令：小车去BB点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(str1 == str32)
	{
		target.pose.position.x = AA_position_x;
		target.pose.position.y = AA_position_y;
		target.pose.orientation.z = AA_orientation_z;
		target.pose.orientation.w = AA_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 12;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKQW.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
        //system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKQW.wav");
		cout<<"好的：小车自主导航至BB点"<<endl;
	}

/***********************************
指令：小车去CC点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(str1 == str33)
	{
		target.pose.position.x = BB_position_x;
		target.pose.position.y = BB_position_y;
		target.pose.orientation.z = BB_orientation_z;
		target.pose.orientation.w = BB_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 13;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKHD.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
        //system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKHD.wav");
		
                cout<<"好的：小车自主导航至CC点"<<endl;
	     
	}

/***********************************
指令：小车去DD点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(str1 == str34)
	{
		target.pose.position.x = FF_position_x;
		target.pose.position.y = FF_position_y;
		target.pose.orientation.z = FF_orientation_z;
		target.pose.orientation.w = FF_orientation_w;
		navigation_auto_pub.publish(target);
                std_msgs::Int8 goal_control_flag_msg;
                goal_control_flag_msg.data = 15;
                goal_control_pub.publish(goal_control_flag_msg);
				OTHER = (char*) "/feedback_voice/OKHD.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
        //system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKHD.wav");
         sleep(6);
                target.pose.position.x = GG_position_x;
		target.pose.position.y = GG_position_y;
		target.pose.orientation.z = GG_orientation_z;
		target.pose.orientation.w = GG_orientation_w;
                navigation_auto_pub.publish(target);
         sleep(6);
                target.pose.position.x = DD_position_x;
		target.pose.position.y = DD_position_y;
		target.pose.orientation.z = DD_orientation_z;
		target.pose.orientation.w = DD_orientation_w;
		navigation_auto_pub.publish(target);
                std_msgs::Int8 cmd_vel_flag_msg;
                cmd_vel_flag_msg.data = 0;
                cmd_vel_flag_pub.publish(cmd_vel_flag_msg);
		cout<<"好的：小车自主导航至DD点"<<endl;
	}

/***********************************
指令：小车去DD点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(str1 == str35)
	{
		target.pose.position.x = DD_position_x;
		target.pose.position.y = DD_position_y;
		target.pose.orientation.z = DD_orientation_z;
		target.pose.orientation.w = DD_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 16;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKHD.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
        //system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKHD.wav");
		cout<<"好的：小车自主导航至DD点"<<endl;
	}
/***********************************
指令：小车去DD点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(str1 == str36)
	{
		target.pose.position.x = EE_position_x;
		target.pose.position.y = EE_position_y;
		target.pose.orientation.z = EE_orientation_z;
		target.pose.orientation.w = EE_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 17;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKHD.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
       // system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKHD.wav");
		cout<<"好的：小车自主导航至DD点"<<endl;
	}

/***********************************
指令：小车去V点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(str1 == str37)
	{
		target.pose.position.x = I_position_x;
		target.pose.position.y = I_position_y;
		target.pose.orientation.z = I_orientation_z;
		target.pose.orientation.w = I_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 1;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKHD.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
       /*system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKB.wav");*/
		cout<<"好的：小车自主导航至V点"<<endl;
	}

/***********************************
指令：小车去V点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(str1 == str38)
	{
		target.pose.position.x = I_position_x;
		target.pose.position.y = I_position_y;
		target.pose.orientation.z = I_orientation_z;
		target.pose.orientation.w = I_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 1;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKHD.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
       /*system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKB.wav");*/
		cout<<"好的：小车自主导航至V点"<<endl;
	}

/***********************************
指令：小车去V点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(str1 == str39)
	{
		target.pose.position.x = I_position_x;
		target.pose.position.y = I_position_y;
		target.pose.orientation.z = I_orientation_z;
		target.pose.orientation.w = I_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 1;
        goal_control_pub.publish(goal_control_flag_msg);
       /*system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKB.wav");*/
		cout<<"好的：小车自主导航至V点"<<endl;
	}

/***********************************
指令：小车去V点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(str1 == str40)
	{
		/*target.pose.position.x = I_position_x;
		target.pose.position.y = I_position_y;
		target.pose.orientation.z = I_orientation_z;
		target.pose.orientation.w = I_orientation_w;
		navigation_auto_pub.publish(target);*/

		/*std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);*/

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 18;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKB.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
       //system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKB.wav");
		cout<<"好的：机械臂向左"<<endl;
	}



/***********************************
指令：小车去V点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(str1 == str41)
	{
		/*target.pose.position.x = I_position_x;
		target.pose.position.y = I_position_y;
		target.pose.orientation.z = I_orientation_z;
		target.pose.orientation.w = I_orientation_w;
		navigation_auto_pub.publish(target);*/

		/*std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);*/

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 19;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKB.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
      // system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKB.wav");
      
		cout<<"好的：机械臂向左"<<endl;
	}



/***********************************
指令：小车去V点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(str1 == str42)
	{
		/*target.pose.position.x = I_position_x;
		target.pose.position.y = I_position_y;
		target.pose.orientation.z = I_orientation_z;
		target.pose.orientation.w = I_orientation_w;
		navigation_auto_pub.publish(target);*/

		/*std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);*/

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 19;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKB.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
      // system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKB.wav");
       sleep(20);
	   OTHER = (char*) "/feedback_voice/HLGN.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
       //system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/HLGN.wav");
		cout<<"好的：机械臂向左"<<endl;
	}
/***********************************
指令：小车去DD点
动作：底盘运动控制器失能(导航控制)，发布目标点
***********************************/
	else if(str1 == str43)
	{
		target.pose.position.x = DD_position_x;
		target.pose.position.y = DD_position_y;
		target.pose.orientation.z = DD_orientation_z;
		target.pose.orientation.w = DD_orientation_w;
		navigation_auto_pub.publish(target);

		std_msgs::Int8 cmd_vel_flag_msg;
        cmd_vel_flag_msg.data = 0;
        cmd_vel_flag_pub.publish(cmd_vel_flag_msg);

        std_msgs::Int8 goal_control_flag_msg;
        goal_control_flag_msg.data = 16;
        goal_control_pub.publish(goal_control_flag_msg);
		OTHER = (char*) "/feedback_voice/OKHD.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
        //system("aplay -D plughw:CARD=Device,DEV=0 ~/wheeltec_robot/src/xf_mic_asr_offline/feedback_voice/OKHD.wav");
		cout<<"好的：小车自主导航至DD点"<<endl;
	}






	/*else if(str1 == str44)
	{
           std_msgs::Int32 msg;
           msg.data= 1;
           ggg_pub.publish(msg);

	OTHER = (char*) "/feedback_voice/OKHD.wav";
	WHOLE = join((head + audio_path),OTHER);
	system(WHOLE);
        
	}*/
/***********************************
辅助指令：失败5次
动作：用户界面打印提醒
***********************************/


	else if(str1 == str12)
	{
		cout<<"您已经连续【输入空指令or识别失败】5次，累计达15次自动进入休眠，输入有效指令后计数清零"<<endl;
	}
/***********************************
辅助指令：失败10次
动作：用户界面打印提醒
***********************************/
	else if(str1 == str13)
	{
		cout<<"您已经连续【输入空指令or识别失败】10次，累计达15次自动进入休眠，输入有效指令后计数清零"<<endl;
	}
/***********************************
辅助指令：遇到障碍物
动作：用户界面打印提醒
***********************************/
	else if(str1 == str14)
	{
		OTHER = (char*) "/feedback_voice/Tracker.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
		cout<<"小车遇到障碍物，已停止运动"<<endl;
	}
/***********************************
辅助指令：小车唤醒
动作：用户界面打印提醒
***********************************/
	else if(str1 == str15)
	{
		OTHER = (char*) "/feedback_voice/awake.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
		cout<<"小车已被唤醒，请说语音指令"<<endl;
		
	}
	

	else if(str1 == str16 && sw == "on")
	{
		sw = "off";
		OTHER = (char*) "/feedback_voice/OK.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
		system("dbus-launch gnome-terminal -- roslaunch simple_follower laserfollow.launch");//打开雷达跟随节点
		cout<<"好的：小车雷达跟随"<<endl;
	}
	else if(str1 == str17 && sw == "on")
	{
		sw = "off";
		OTHER = (char*) "/feedback_voice/OK.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
		system("rosnode kill /laser_tracker");//关闭雷达避障
		system("dbus-launch gnome-terminal -- roslaunch xf_mic_asr_offline_circle voi_visual_follower.launch");//打开视觉跟随节点
		
		cout<<"好的：小车色块跟随"<<endl;
	}
	else if(str1 == str18 && sw == "off")
	{
		sw = "on";
		OTHER = (char*) "/feedback_voice/OK.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
		cout<<"好的：关闭雷达跟随"<<endl;
	}
	else if(str1 == str19 && sw == "off")
	{
		sw = "on";
		OTHER = (char*) "/feedback_voice/OK.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
		cout<<"好的：关闭色块跟随"<<endl;

	}
	else if(str1 == str20 && sw == "on")
	{
		sw = "off";
		OTHER = (char*) "/feedback_voice/OK.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
		system("rosnode kill /amcl");//关闭amcl定位节点
		system("rosnode kill /map_server_for_test");//关闭地图保存节点
		system("rosnode kill /move_base");//关闭/move_base节点
		system("dbus-launch gnome-terminal -- roslaunch xf_mic_asr_offline_circle voi_rrt_slam.launch");//打开自主建图功能
		cout<<"好的：打开自主建图"<<endl;

	}
	else if(str1 == str21 && sw == "off")
	{
		sw = "on";
		OTHER = (char*) "/feedback_voice/OK.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
		cout<<"好的：关闭自主建图"<<endl;
	}
	else if(str1 == str22)
	{
		OTHER = (char*) "/feedback_voice/OK.wav";
		WHOLE = join((head + audio_path),OTHER);
		system(WHOLE);
		system("rosnode kill /move_base");
		system("rosnode kill /amcl");
		system("rosnode kill /map_server_for_test");
		system("dbus-launch gnome-terminal -- roslaunch xf_mic_asr_offline_circle voi_navigation.launch");//打开导航节点
		system("dbus-launch gnome-terminal -- roslaunch xf_mic_asr_offline_circle send_mark.launch");//打开多点巡航脚本
		cout<<"好的：小车开始导航"<<endl;
	}
}

/**************************************************************************
函数功能：寻找语音开启成功标志位sub回调函数
入口参数：voice_flag_msg  voice_control.cpp
返回  值：无
**************************************************************************/
void voice_flag_Callback(std_msgs::Int8 msg)
{
	voice_flag = msg.data;
	OTHER = (char*) "/feedback_voice/voice_control.wav";
	WHOLE = join((head + audio_path),OTHER);
	if(voice_flag == 1)
	{
		system(WHOLE);
		cout<<"语音打开成功"<<endl;
	}

}

/*
void kill_pro(char pro_name[])
{
	char get_pid[30] = "pidof ";
	strcat(get_pid,pro_name);
	FILE *fp = popen(get_pid,"r");
	
	char pid[10] = {0};
	fgets(pid,10,fp);
	pclose(fp);
	
	char cmd[20] = "kill -9 ";
	strcat(cmd,pid);
	system(cmd);
	
}
*/

/**************************************************************************
函数功能：主函数
入口参数：无
返回  值：无
**************************************************************************/
int main(int argc, char** argv)
{
        
        ros::init(argc, argv, "xrtx");
	ros::init(argc, argv, "cmd_rec");     //初始化ROS节点
        ros::init(argc, argv, "my_subscriber_node");
	ros::NodeHandle n;    //创建句柄
	ros::NodeHandle gg;
	

    ros::NodeHandle nh;

 // 订阅四个话题
    ros::Subscriber sub1 = nh.subscribe("serial_data1", 1000, serialData1Callback);
    ros::Subscriber sub2 = nh.subscribe("serial_data2", 1000, serialData2Callback);
    ros::Subscriber sub3 = nh.subscribe("serial_data3", 1000, serialData3Callback);
    ros::Subscriber sub4 = nh.subscribe("serial_data4", 1000, serialData4Callback);
    ggg_pub = gg.advertise<std_msgs::Int32>("ggg",1000);
	string if_akm;
	
	/***创建寻找声源标志位话题发布者***/
	follow_flag_pub = n.advertise<std_msgs::Int8>("follow_flag",1);

	/***创建I、J、K点到达标志位话题发布者***/
	goal_control_pub = n.advertise<std_msgs::Int8>("goal_control_flag",1);

	/***创建底盘运动控制器标志位话题发布者***/
	cmd_vel_flag_pub = n.advertise<std_msgs::Int8>("cmd_vel_flag",1);

	/***创建底盘运动话题发布者***/
	vel_pub = n.advertise<geometry_msgs::Twist>("ori_vel",1);

	cmd_vel_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 1);

	/***创建唤醒标志位话题发布者***/
	awake_flag_pub = n.advertise<std_msgs::Int8>("awake_flag", 1);

  /***创建自主导航目标点话题发布者***/
	navigation_auto_pub = n.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal",1);

	/***创建离线命令词识别结果话题订阅者***/
	ros::Subscriber voice_words_sub = n.subscribe("voice_words",1,voice_words_callback);

	/***创建寻找语音开启标志位话题订阅者***/
	ros::Subscriber voice_flag_sub = n.subscribe("voice_flag", 1, voice_flag_Callback);

	n.param("/command_recognition/audio_path", audio_path, std::string("~/catkin_ws1111/src/xf_mic_asr_offline/feedback_voice"));


	n.param<float>("/I_position_x", I_position_x, 1);
	n.param<float>("/I_position_y", I_position_y, 0);
	n.param<float>("/I_orientation_z", I_orientation_z, 0);
	n.param<float>("/I_orientation_w", I_orientation_w, 1);
	n.param<float>("/J_position_x", J_position_x, 2);
	n.param<float>("/J_position_y", J_position_y, 0);
	n.param<float>("/J_orientation_z", J_orientation_z, 0);
	n.param<float>("/J_orientation_w", J_orientation_w, 1);
	n.param<float>("/K_position_x", K_position_x, 3);
	n.param<float>("/K_position_y", K_position_y, 0);
	n.param<float>("/K_orientation_z", K_orientation_z, 0);
	n.param<float>("/K_orientation_w", K_orientation_w, 1);
	n.param<float>("/Y_position_x", Y_position_x, 4);
	n.param<float>("/Y_position_y", Y_position_y, 0);
	n.param<float>("/Y_orientation_z", Y_orientation_z, 0);
	n.param<float>("/Y_orientation_w", Y_orientation_w, 1);
	n.param<float>("/Z_position_x", Z_position_x, 5);
	n.param<float>("/Z_position_y", Z_position_y, 0);
	n.param<float>("/Z_orientation_z", Z_orientation_z, 0);
	n.param<float>("/Z_orientation_w", Z_orientation_w, 1);
	n.param<float>("/W_position_x", W_position_x, 6);
	n.param<float>("/W_position_y", W_position_y, 0);
	n.param<float>("/W_orientation_z", W_orientation_z, 0);
	n.param<float>("/W_orientation_w", W_orientation_w, 1);
	n.param<float>("/R_position_x", R_position_x, 7);
	n.param<float>("/R_position_y", R_position_y, 0);
	n.param<float>("/R_orientation_z", R_orientation_z, 0);
	n.param<float>("/R_orientation_w", R_orientation_w, 1);
	n.param<float>("/S_position_x", S_position_x, 8);
	n.param<float>("/S_position_y", S_position_y, 0);
	n.param<float>("/S_orientation_z", S_orientation_z, 0);
	n.param<float>("/S_orientation_w", S_orientation_w, 1);
	n.param<float>("/T_position_x", T_position_x, 9);
	n.param<float>("/T_position_y", T_position_y, 0);
	n.param<float>("/T_orientation_z", T_orientation_z, 0);
	n.param<float>("/T_orientation_w", T_orientation_w, 1);
	n.param<float>("/U_position_x", U_position_x, 1);
	n.param<float>("/U_position_y", U_position_y, 0);
	n.param<float>("/U_orientation_z", U_orientation_z, 0);
	n.param<float>("/U_orientation_w", U_orientation_w, 1);
	n.param<float>("/V_position_x", V_position_x, 2);
	n.param<float>("/V_position_y", V_position_y, 0);
	n.param<float>("/V_orientation_z", V_orientation_z, 0);
	n.param<float>("/V_orientation_w", V_orientation_w, 1);
    n.param<float>("/AA_position_x", AA_position_x, 2);
	n.param<float>("/AA_position_y", AA_position_y, 0);
	n.param<float>("/AA_orientation_z", AA_orientation_z, 0);
	n.param<float>("/AA_orientation_w", AA_orientation_w, 1);
    n.param<float>("/BB_position_x", BB_position_x, 2);
	n.param<float>("/BB_position_y", BB_position_y, 0);
	n.param<float>("/BB_orientation_z", BB_orientation_z, 0);
	n.param<float>("/BB_orientation_w", BB_orientation_w, 1);
    n.param<float>("/CC_position_x", CC_position_x, 2);
	n.param<float>("/CC_position_y", CC_position_y, 0);
	n.param<float>("/CC_orientation_z", CC_orientation_z, 0);
	n.param<float>("/CC_orientation_w", CC_orientation_w, 1);
    n.param<float>("/DD_position_x", DD_position_x, 2);
	n.param<float>("/DD_position_y", DD_position_y, 0);
	n.param<float>("/DD_orientation_z", DD_orientation_z, 0);
	n.param<float>("/DD_orientation_w", DD_orientation_w, 1);
    n.param<float>("/EE_position_x", EE_position_x, 2);
	n.param<float>("/EE_position_y", EE_position_y, 0);
	n.param<float>("/EE_orientation_z", EE_orientation_z, 0);
	n.param<float>("/EE_orientation_w", EE_orientation_w, 1);
    n.param<float>("/FF_position_x", FF_position_x, 2);
	n.param<float>("/FF_position_y", FF_position_y, 0);
	n.param<float>("/FF_orientation_z", FF_orientation_z, 0);
	n.param<float>("/FF_orientation_w", FF_orientation_w, 1);
    n.param<float>("/GG_position_x", GG_position_x, 2);
	n.param<float>("/GG_position_y", GG_position_y, 0);
	n.param<float>("/GG_orientation_z", GG_orientation_z, 0);
	n.param<float>("/GG_orientation_w", GG_orientation_w, 1);
	n.param<float>("/line_vel_x", line_vel_x, 0.2);
	n.param<float>("/ang_vel_z", ang_vel_z, 0.2);
	n.param("/if_akm_yes_or_no", if_akm, std::string("no"));

	if(if_akm == "yes")
		turn_line_vel_x = 0.2;
	else 
		turn_line_vel_x = 0;

	/***自主导航目标点数据初始化***/
	target.header.seq = 0;
	//target.header.stamp;
	target.header.frame_id = "map";
	target.pose.position.x = 0;
	target.pose.position.y = 0;
	target.pose.position.z = 0;
	target.pose.orientation.x = 0;
	target.pose.orientation.y = 0;
	target.pose.orientation.z = 0;
	target.pose.orientation.w = 1;


  /***用户界面***/
	sleep(7);
	cout<<"您可以语音控制啦！以降噪板设置的唤醒词为准[默认:“”]"<<endl;
	cout<<"小车前进———————————>向前"<<endl;
	cout<<"小车后退———————————>后退"<<endl;
	cout<<"小车左转———————————>左转"<<endl;
	cout<<"小车右转———————————>右转"<<endl;
	cout<<"小车停———————————>停止"<<endl;
	cout<<"小车休眠———————————>休眠，等待下一次唤醒"<<endl;
	cout<<"小车过来———————————>寻找声源"<<endl;
	cout<<"小车去I点———————————>小车自主导航至I点"<<endl;
	cout<<"小车去J点———————————>小车自主导航至J点"<<endl;
	cout<<"小车去K点———————————>小车自主导航至K点"<<endl;
	cout<<"小车雷达跟随———————————>小车打开雷达跟随"<<endl;
	cout<<"关闭雷达跟随———————————>小车关闭雷达跟随"<<endl;
	cout<<"小车色块跟随———————————>小车打开色块跟随"<<endl;
	cout<<"关闭色块跟随———————————>小车关闭色块跟随"<<endl;
	cout<<"打开自主建图———————————>小车打开自主建图"<<endl;
	cout<<"关闭自主建图———————————>关闭打开自主建图"<<endl;
	cout<<"开始导航———————————>小车开始导航"<<endl;
	cout<<"关闭导航———————————>小车关闭导航"<<endl;
	cout<<"\n"<<endl;
	//printf("-----turn_line_vel_x =%f\n",turn_line_vel_x);
	//printf("-----I_position_x =%f\n",I_position_x);
	//printf("-----I_position_y =%f\n",I_position_y);
	//printf("-----I_orientation_z =%f\n",I_orientation_z);
	//printf("-----I_orientation_w =%f\n",I_orientation_w);
	//printf("-----J_position_x =%f\n",J_position_x);
	//printf("-----J_position_y =%f\n",J_position_y);
	//printf("-----J_orientation_z =%f\n",J_orientation_z);
	//printf("-----J_orientation_w =%f\n",J_orientation_w);
	//printf("-----K_position_x =%f\n",K_position_x);
	//printf("-----K_position_y =%f\n",K_position_y);
	//printf("-----K_orientation_z =%f\n",K_orientation_z);
	//printf("-----K_orientation_w =%f\n",K_orientation_w);
	//printf("-----line_vel_x =%f\n",line_vel_x);
	//printf("-----ang_vel_z =%f\n",ang_vel_z);
	ros::spin();
}





//小车色块跟随,关闭色块跟随
